#include <system.h>
/**************************************************************//*
Interface Timer{
	Provides{
		command TIMER_init();
		command TIMER_reg(timer_t*,void(*timerexpired)());
		command TIMER_start(timer_t*);
		command TIMER_startPeriodic(timer_t*);
		command TIMER_stop(timer_t*);	
	}
	Uses{
		signal (*timerexpiredOneshot)();
		signal (*timerexpiredPeriodic)();
	}
}*/
/***************************************************************/
//PRIVATE:
	//Data Member:
	struct {
  		uint32_t clock;
  		timer_t *p_start;
	} timer_prop;
	//Function Member:
	void TIMER_insert(timer_t *t);
	void TIMER_remove(timer_t *t);
	void TIMER_expired(timer_t *t);
	void Timer_isr(void);
	
/**************************************************************/
/*            Client Function Implementation                  */
/*                      (Provides)                            */					
/**************************************************************/
void TIMER_reg(timer_t *t, void (*run)(void)) {

  uint8_t sreg = SREG;
  cli();

  t->started = FALSE;
  t->periodic = FALSE;

  t->run = run;

  SREG = sreg;

}

void TIMER_start(timer_t *t, uint16_t time) {

  if (t->started) {
    TIMER_remove(t);
  }

  uint8_t sreg = SREG;
  cli();

  t->started = TRUE;
  t->periodic = FALSE;

  t->period = time;

  t->expired = timer_prop.clock + time;

  SREG = sreg;

  TIMER_insert(t);

}

void TIMER_startPeriodic(timer_t *t, uint16_t time) {

  if (t->started) {
    TIMER_remove(t);
  }

  uint8_t sreg = SREG;
  cli();

  t->started = TRUE;
  t->periodic = TRUE;

  t->period = time;

  t->expired = timer_prop.clock + time;

  SREG = sreg;

  TIMER_insert(t);

}

void TIMER_stop(timer_t *t) {

  t->started = FALSE;
  t->periodic = FALSE;

  TIMER_remove(t);

}
/**************************************************************/
/*            Private Function Implementation                 */
/*                      (Internals)                           */					
/**************************************************************/
void TIMER_expired(timer_t *t) {

  if (t->periodic) {
    uint8_t sreg = SREG;
	cli();

	t->expired = timer_prop.clock + t->period;

	SREG = sreg;

	TIMER_insert(t);
  }
  else {
    t->started = FALSE;
	t->periodic = FALSE;
  }

  void (*run)(void) = t->run;
  (*run)();

}
void TIMER_insert(timer_t *t) {

  uint8_t sreg = SREG;
  cli();

  if (timer_prop.p_start == NULL) {
    timer_prop.p_start = t;

	t->prev = NULL;
	t->next = NULL;

	SREG = sreg;
	return;
  }

  timer_t *a = timer_prop.p_start;
  timer_t *last;

  while (a != NULL) {
    last = a;
	a = a->next;
  }

  last->next = t;

  t->prev = last;
  t->next = NULL;

  SREG = sreg;

}
void TIMER_remove(timer_t *t) {

  uint8_t sreg = SREG;
  cli();

  if (timer_prop.p_start == t) {
    timer_prop.p_start = t->next;

	if (t->next != NULL)
	  (t->next)->prev = NULL; 
							  
	t->prev = NULL;
	t->next = NULL;

    SREG = sreg;
	return;
  }

  if (t->prev != NULL)
    (t->prev)->next = t->next;

  if (t->next != NULL)
    (t->next)->prev = t->prev;

  t->prev = NULL;
  t->next = NULL;

  SREG = sreg;

}
/**************************************************************/
/*            System Function Implementation                  */
/*                    (Used by system)                        */					
/**************************************************************/
void TIMER_init() {
  timer_prop.p_start = NULL;
  timer_prop.clock = 0;

  DRIVER_CLOCK_INIT();
  DRIVER_CLOCK_REG(Timer_isr);
}
void TIMER_run() {

  if (timer_prop.p_start == NULL) {
    return;
  }

  timer_t *t = timer_prop.p_start;

  while (t != NULL) {
    uint8_t sreg = SREG;
	cli();

    if (t->expired <= timer_prop.clock) {
	  SREG = sreg;

      TIMER_remove(t);
      TIMER_expired(t);

	  break;
	}

    t = t->next;
	SREG = sreg;
  }

}
void Timer_isr (void){

  timer_prop.clock++;

}
